// 串口调试工具 - 用于诊断单片机通信问题
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <iostream>
#include <iomanip>
#include <sstream>

class SerialDebugTool {
public:
    SerialDebugTool(ros::NodeHandle& nh) : nh_(nh) {
        // 订阅下行数据（发送到MCU的数据）
        downstream_sub_ = nh_.subscribe("/serial/downstream", 10, 
            &SerialDebugTool::downstreamCallback, this);
        
        // 发布测试数据
        test_pub_ = nh_.advertise<std_msgs::String>("/serial/downstream", 10);
        
        ROS_INFO("串口调试工具已启动");
        ROS_INFO("监听话题: /serial/downstream");
        ROS_INFO("输入命令:");
        ROS_INFO("  1 - 发送简单字符串测试");
        ROS_INFO("  2 - 发送JSON测试数据");
        ROS_INFO("  3 - 发送ASCII可见字符测试");
        ROS_INFO("  4 - 发送带换行符的数据");
        ROS_INFO("  5 - 发送二进制数据测试");
        ROS_INFO("  q - 退出");
    }
    
    void run() {
        ros::Rate rate(10);
        
        while (ros::ok()) {
            std::cout << "\n请输入命令 (1-5, q退出): ";
            
            char cmd;
            std::cin >> cmd;
            
            switch (cmd) {
                case '1':
                    sendSimpleTest();
                    break;
                case '2':
                    sendJsonTest();
                    break;
                case '3':
                    sendAsciiTest();
                    break;
                case '4':
                    sendNewlineTest();
                    break;
                case '5':
                    sendBinaryTest();
                    break;
                case 'q':
                case 'Q':
                    ROS_INFO("退出调试工具");
                    return;
                default:
                    std::cout << "无效命令，请重新输入\n";
                    break;
            }
            
            ros::spinOnce();
            rate.sleep();
        }
    }

private:
    ros::NodeHandle& nh_;
    ros::Subscriber downstream_sub_;
    ros::Publisher test_pub_;
    
    void downstreamCallback(const std_msgs::String::ConstPtr& msg) {
        std::string data = msg->data;
        
        ROS_INFO("=== 检测到下行数据 ===");
        ROS_INFO("数据长度: %lu 字节", data.length());
        ROS_INFO("原始字符串: %s", data.c_str());
        
        // 十六进制显示
        std::stringstream hexStream;
        for (unsigned char c : data) {
            hexStream << "\\x" << std::hex << std::uppercase << std::setw(2) 
                     << std::setfill('0') << static_cast<int>(c);
        }
        ROS_INFO("十六进制表示: %s", hexStream.str().c_str());
        
        // ASCII码显示
        std::stringstream asciiStream;
        for (unsigned char c : data) {
            if (c >= 32 && c <= 126) {
                asciiStream << c;
            } else {
                asciiStream << "[" << static_cast<int>(c) << "]";
            }
        }
        ROS_INFO("ASCII显示: %s", asciiStream.str().c_str());
        
        ROS_INFO("=====================");
    }
    
    void sendSimpleTest() {
        std_msgs::String msg;
        msg.data = "HELLO";
        test_pub_.publish(msg);
        ROS_INFO("发送简单测试: HELLO");
    }
    
    void sendJsonTest() {
        std_msgs::String msg;
        msg.data = "{\"test_key\":123}";
        test_pub_.publish(msg);
        ROS_INFO("发送JSON测试: {\"test_key\":123}");
    }
    
    void sendAsciiTest() {
        std_msgs::String msg;
        msg.data = "ABC123!@#";
        test_pub_.publish(msg);
        ROS_INFO("发送ASCII测试: ABC123!@#");
    }
    
    void sendNewlineTest() {
        std_msgs::String msg;
        msg.data = "TEST\n";
        test_pub_.publish(msg);
        ROS_INFO("发送换行测试: TEST\\n");
    }
    
    void sendBinaryTest() {
        std_msgs::String msg;
        // 构造包含各种字节值的测试数据
        msg.data = std::string("\x01\x02\x03\xFF\xFE\xFD", 6);
        test_pub_.publish(msg);
        ROS_INFO("发送二进制测试: \\x01\\x02\\x03\\xFF\\xFE\\xFD");
    }
};

int main(int argc, char** argv) {
    setlocale(LC_ALL, "zh_CN.UTF-8");
    
    ros::init(argc, argv, "serial_debug_tool");
    ros::NodeHandle nh;
    
    ROS_INFO("=== 串口调试工具 ===");
    ROS_INFO("此工具用于诊断MCU通信问题");
    ROS_INFO("请确保 serialport_node 正在运行");
    
    try {
        SerialDebugTool tool(nh);
        tool.run();
    } catch (const std::exception& e) {
        ROS_ERROR("调试工具异常: %s", e.what());
        return -1;
    }
    
    return 0;
}
